/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// -*- mode: c++ -*- 
/**
 * \file
 * \brief Ceilbot - header.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#ifndef _FSRSIM_CEILBOT_H_
#define _FSRSIM_CEILBOT_H_

#include "Object.h"
#include "Sick.h"
#include "Shapes.h"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlTypes.hpp"
#include "../MaCI/interfaces/Position/PositionServer.hpp"
#include "../MaCI/interfaces/Position/PositionTypes.hpp"
#include "../MaCI/interfaces/Energy/EnergyServer.hpp"
#include "../MaCI/interfaces/CoordinateDrive/CoordinateDriveServer.hpp"

#include <string>

class CCeilbot : public CObject
{
public:
  
  CCeilbot(CWorld* World,
           const std::string &aGIMnetName,
           const std::string &aMaCIGroup,
           const float &x, const float &y, 
           const float &w, const float &h, const float &l);
  
  void Activate();

  void Update(float StepSize);

  void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);
    
  void Draw();
    
  static CObject* LoadMLMA(CWorld *world, xmlNode *node);
    
private: // Variables
  gimi::GIMI gimi;
  MaCI::MaCICtrl::CMaCICtrlServer mcs;
  MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
  MaCI::Position::CPositionServer PositionServer;
  MaCI::Energy::CEnergyServer EnergyServer;
  MaCI::CoordinateDrive::CCoordinateDriveServer CoordinateDriveServer;
    
  std::string gimnetName;
  std::string MaCIGroup;
  float speed, angularSpeed, posX, posY, heading;
  float positionTimer, energyTimer;

  // The coordinates for our route
  std::vector<MaCI::CoordinateDrive::TCoordinate2D> iCoordinates;
    
  // The drive mode. (0 = stop, 1 = start
  int iDrive;


  CBox *chassis;
  CSick *LaserScanner;
  TMaterial chassisMaterial;
  TMaterial chassis2Material;

private: // Functions
  void connectGIMI();
  
  // This functions drives the bot according to the coordinates
  void CoordDriver();
  void GenerateEnergyEvent(void);
};

#endif
